#include "rclcpp/rclcpp.hpp"

#include "sen_ug_ctr/pid.h"
#include "sen_ug_ctr/user_lib.h"
#include "sen_ug_ctr/struct_typedef.h"

#include "auto_aim_interfaces/msg/dangle_shoot.hpp"
#include "auto_aim_interfaces/msg/cmd_up_yaw_volt_shoot.hpp"
#include "std_msgs/msg/u_int16.hpp"
#include <mutex>
#include "sensor_msgs/msg/imu.hpp"

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <cmath>

#define GIMBAL_TASK_CTRL_TIME 2 // 任务控制时间间隔，单位ms

#define SHOW_ECD
#define MIDDLE_ECD 1000
#define ECD_FULL_ROUND 8192 // 0-8191
#define MAX_ANGLE_OFFSET (M_PI / 3)
// dejavu now, 2024.4.2 00:59

// #define YAW_ASSEM_DIR 1
#define YAW_ASSEM_DIR (-1) // 倒装的GM6020 


// 云台左转为yaw角度正方向
// 云台右转为yaw角度负方向
// 电机倒立安装，云台左转时，会导致电机ECD减小；云台右转时，会导致ECD增大
#define YAW_ECD_LEFT_EXTREME (MIDDLE_ECD - )

#define PITCH_TEST_RAD_MIN (-0.4f)
#define PITCH_TEST_RAD_MAX (0.48f)

/**********************PID控制参数********************/

#define PITCH_AGL_KP 15.0f
#define PITCH_AGL_KI 0.0f
#define PITCH_AGL_KD 0.0f
#define PITCH_AGL_SPD_MAX_OUT (30.0f)
#define PITCH_AGL_SPD_MAX_IOUT (0.7f)

#define PITCH_SPD_KP 1200.0f
#define PITCH_SPD_KI 0.5f
#define PITCH_SPD_KD 3.0f
#define PITCH_VOLT_MAX_OUT 6000.0f
#define PITCH_VOLT_MAX_IOUT 40.0f

#define YAW_AGL_KP 25.0f
#define YAW_AGL_KI 0.0f
#define YAW_AGL_KD 0.0f
#define YAW_AGL_SPD_MAX_OUT (80.0f)
#define YAW_AGL_SPD_MAX_IOUT (0.7f)

#define YAW_SPD_KP 110000.0f
#define YAW_SPD_KI 0000.0f
#define YAW_SPD_KD 000.0f
#define YAW_VOLT_MAX_OUT 25000.0f // maximum for GM6020 is 25000
#define YAW_VOLT_MAX_IOUT 5000.0f

#define PID_DEATH_AREA 0.005 //防止I不断累积

// 一阶滤波器的参数,专门给。这两个值越大，理论上越丝滑
#define PITCH_SPD_FILTER_NUM 15.0f
#define YAW_SPD_FILTER_NUM 20.0f

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

#define range_limit_inside(toBeLimited, range) \
    {                                          \
        if ((toBeLimited) > (range))           \
        {                                      \
            (toBeLimited) = (range);           \
        }                                      \
        else if ((toBeLimited) < -(range))     \
        {                                      \
            (toBeLimited) = -(range);          \
        }                                      \
    }

struct gimbalMotorCtrl_t
{
    // rads
    const fp32 *anglePoint;   // 从姿态解算任务得到的角度位置
    const fp32 *gyroPointer;  // 陀螺仪角速度指针
    fp32 nowAbsoluteAngle;    // 当前姿态角
    fp32 radSpeed;            // 角速度 rad/s
    fp32 wantedAbsoluteAngle; // 目标姿态角

    // PIDs
    pid_type_def spdPid; // 用于控制速度的PID环。输入角速度 rad/s, 输出GM6020电压 int16
    pid_type_def aglPid; // 用于控制角度的PID环。输入角度 rad,输出角速度rad/s

    // 控制量
    int16_t giveVolt; // GM6020发送电压
    // uint8_t zeroVoltMark;

    // 一阶滤波器，过滤角速度
    first_order_filter_type_t spd_filter;
};

struct gimbalMotorCtrl_t gimbalPitchCtrl, gimbalYawCtrl;
struct gimbalMotorCtrl_t *gimbalCtrl[2] = {&gimbalPitchCtrl, &gimbalYawCtrl};

fp32 yawSpdPID[3] = {YAW_SPD_KP, YAW_SPD_KI, YAW_SPD_KD};
fp32 pitchSpdPID[3] = {PITCH_SPD_KP, PITCH_SPD_KI, PITCH_SPD_KD};
fp32 yawAglPID[3] = {YAW_AGL_KP, YAW_AGL_KI, YAW_AGL_KD};
fp32 pitchAglPID[3] = {PITCH_AGL_KP, PITCH_AGL_KI, PITCH_AGL_KD};

class SenUpCtrNode : public rclcpp::Node
{
public:
    SenUpCtrNode()
    : Node("sen_up_ctr_node")
    {
        // 这是各种初始化的操作
        init_gimbal_ctrls();

        // 定时调用函数
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(GIMBAL_TASK_CTRL_TIME), std::bind(&SenUpCtrNode::timer_callback, this));

        dangle_shoot_subscription_ = this->create_subscription<auto_aim_interfaces::msg::DangleShoot>(
            "dangle_shoot", 10, std::bind(&SenUpCtrNode::dangle_shoot_msg_message_callback, this, std::placeholders::_1));
        
        imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "imu", 10, std::bind(&SenUpCtrNode::imu_message_callback, this, std::placeholders::_1));

        cmd_up_yaw_volt_shoot_publisher_ = this->create_publisher<auto_aim_interfaces::msg::CmdUpYawVoltShoot>("cmd_up_yaw_volt_shoot", 10);

    

        this->declare_parameter<double>("roll", 0.0);
        this->declare_parameter<double>("pitch", 0.0);
        this->declare_parameter<double>("yaw", 0.0);
        double roll, pitch, yaw;
        // 此处的rpy是gimbal_link下的imu的rotation
        this->get_parameter("roll", roll);
        this->get_parameter("pitch", pitch);
        this->get_parameter("yaw", yaw);

        q_gl_imu.setRPY(roll, pitch, yaw);
        // log
        RCLCPP_INFO(this->get_logger(), "q_gl_imu: %f %f %f %f", q_gl_imu.getX(), q_gl_imu.getY(), q_gl_imu.getZ(), q_gl_imu.getW());
        // 取逆
        q_imu_gl.setX(-q_gl_imu.getX());
        q_imu_gl.setY(-q_gl_imu.getY());
        q_imu_gl.setZ(-q_gl_imu.getZ());
        q_imu_gl.setW(q_gl_imu.getW());
        q_imu_gl.normalize();

    }

private:


    void init_gimbal_ctrls(void)
    {

        // 初始化PID参数
        // 两个双环PID不共用参数
        PID_init(&(gimbalPitchCtrl.spdPid), PID_POSITION, pitchSpdPID, PITCH_VOLT_MAX_OUT, PITCH_VOLT_MAX_IOUT);
        PID_init(&(gimbalYawCtrl.spdPid), PID_POSITION, yawSpdPID, YAW_VOLT_MAX_OUT, YAW_VOLT_MAX_IOUT);

        PID_init(&(gimbalPitchCtrl.aglPid), PID_POSITION, pitchAglPID, PITCH_AGL_SPD_MAX_OUT, PITCH_AGL_SPD_MAX_IOUT);
        PID_init(&(gimbalYawCtrl.aglPid), PID_POSITION, yawAglPID, YAW_AGL_SPD_MAX_OUT, YAW_AGL_SPD_MAX_IOUT);

        // 滤波初始化
        const static fp32 gimbal_pitch_order_filter[1] = {PITCH_SPD_FILTER_NUM};
        const static fp32 gimbal_yaw_order_filter[1] = {YAW_SPD_FILTER_NUM};

        first_order_filter_init(&(gimbalPitchCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_pitch_order_filter);
        first_order_filter_init(&(gimbalYawCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_yaw_order_filter);

        // 初始化当前角度和目标角度、pid参数、里程碑栈法数据
        for (int i = 0; i < 2; i++)
        {
            // (gimbalCtrl[i])->zeroVoltMark = 0;
            (gimbalCtrl[i])->nowAbsoluteAngle = *((gimbalCtrl[i])->anglePoint);
            gimbalCtrl[i]->wantedAbsoluteAngle = (gimbalCtrl[i])->nowAbsoluteAngle;
        }
        PID_clear(&gimbalYawCtrl.aglPid);
        PID_clear(&gimbalYawCtrl.spdPid);
        PID_clear(&gimbalPitchCtrl.aglPid);
        PID_clear(&gimbalPitchCtrl.spdPid);
        gimbalYawCtrl.radSpeed = 0;
        gimbalPitchCtrl.radSpeed = 0;
    }

    fp32 radFormat(fp32 rawAngle)
    {
        while (rawAngle > M_PI)
            rawAngle -= (2 * M_PI);
        while (rawAngle < (-M_PI))
            rawAngle += (2 * M_PI);
        return rawAngle;
    }

    fp32 gimbal_PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
    {
        if (pid == NULL)
        {
            return 0.0f;
        }

        pid->error[2] = pid->error[1];
        pid->error[1] = pid->error[0];
        pid->set = set;
        pid->fdb = ref;
        pid->error[0] = radFormat(set - ref);
        if (pid->mode == PID_POSITION)
        {
            pid->Pout = pid->Kp * pid->error[0];
            pid->Iout += pid->Ki * pid->error[0];
            pid->Dbuf[2] = pid->Dbuf[1];
            pid->Dbuf[1] = pid->Dbuf[0];
            pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
            pid->Dout = pid->Kd * pid->Dbuf[0];
            range_limit_inside(pid->Iout, pid->max_iout);
            pid->out = pid->Pout + pid->Iout + pid->Dout;
            range_limit_inside(pid->out, pid->max_out);
        }
        else if (pid->mode == PID_DELTA)
        {
            pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
            pid->Iout = pid->Ki * pid->error[0];
            pid->Dbuf[2] = pid->Dbuf[1];
            pid->Dbuf[1] = pid->Dbuf[0];
            pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
            pid->Dout = pid->Kd * pid->Dbuf[0];
            pid->out += pid->Pout + pid->Iout + pid->Dout;
            range_limit_inside(pid->out, pid->max_out);
        }
        return pid->out;
    }

    void refresh_angle_states(void)
    {
        // 当前姿态角和角速度
        gimbalYawCtrl.nowAbsoluteAngle = yaw_;
        gimbalYawCtrl.radSpeed = v_yaw_;
        gimbalPitchCtrl.nowAbsoluteAngle = pitch_;
        gimbalPitchCtrl.radSpeed = v_pitch_;
    }

    void get_control_angles(void)
    {
        std::lock_guard<std::mutex> lock(want_mutex_);
        gimbalYawCtrl.wantedAbsoluteAngle = want_yaw_;
        gimbalPitchCtrl.wantedAbsoluteAngle = want_pitch_ ;
    }

    void limit_angles()
    {
        struct gimbalMotorCtrl_t *c;
        int i;
        for (i = 0; i < 2; i++)
        {
            c = gimbalCtrl[i];
            fp32 near_rotate = radFormat(c->wantedAbsoluteAngle - c->nowAbsoluteAngle);
            if (near_rotate > (M_PI / 2))
                c->wantedAbsoluteAngle = c->nowAbsoluteAngle + (M_PI / 2);
            if (near_rotate < (-M_PI / 2))
                c->wantedAbsoluteAngle = c->nowAbsoluteAngle - (M_PI / 2);
        }
        fp32 want_near_pitch = radFormat(gimbalPitchCtrl.wantedAbsoluteAngle);
        if (want_near_pitch > PITCH_TEST_RAD_MAX)
            gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MAX;
        if (want_near_pitch < PITCH_TEST_RAD_MIN)
            gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MIN;

        // limit ecd for gimbal

        // 计算还能够旋转的最大yaw轴角度
        // 由于GM6020在云台上是倒立安装的，电机ECD减小，对应云台左转，yaw增加
        // 计算还能向左转的角度，限制幅度

        // 以2*PI为模，才能保留旋转方向。
        // float result = std::fmod(, 2*M_PI);
        float angle_elapesed;

        {
        std::lock_guard<std::mutex> lock(ecd_yaw_mutex_);
        angle_elapesed = radFormat((ecd_yaw_ - MIDDLE_ECD) * 2 * M_PI / ECD_FULL_ROUND * YAW_ASSEM_DIR);
        }

        float posi_more_angle = MAX_ANGLE_OFFSET - angle_elapesed;
        float nega_more_angle = -MAX_ANGLE_OFFSET - angle_elapesed;
        float want_near_yaw = radFormat(gimbalYawCtrl.wantedAbsoluteAngle - gimbalYawCtrl.nowAbsoluteAngle);
        if (want_near_yaw > posi_more_angle) {
            gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + posi_more_angle;
        }
        if (want_near_yaw < nega_more_angle) {
            gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + nega_more_angle;
        }
    }

    void calc_PID(void)
    {
        uint8_t i;
        struct gimbalMotorCtrl_t *c;
        // #ifndef SENTRY
        for (i = 0; i < 2; i++)
        {
            c = gimbalCtrl[i];
            gimbal_PID_calc(&(c->aglPid), c->nowAbsoluteAngle, c->wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
            PID_calc(&(c->spdPid), c->radSpeed, (c->aglPid).out);                       // 普通的速度控制环
            c->giveVolt = c->spdPid.out;                                                // 给电压
            // if (c->zeroVoltMark == 1)
            // {
            //     c->giveVolt = 0;
            // }
        }
    }

    void timer_callback()
    {
        // 在这里执行你的操作

        refresh_angle_states(); // 从传感器获得当前角度数据
        // 这些信息是订阅得到的信息，实时更新在节点的成员变量中
        
        get_control_angles();   // 从遥控器获得目标角度
        // 实际上是从订阅得到的信息中实时更新的目标角度。通过mutex保证互斥访问

        limit_angles();
        // 统一进行的角度限制

        calc_PID(); // 总是控制角度，双环

        // 发送
        // 发布一次话题。在CAN serial话题中，订阅这个话题，并将数据发送到CAN总线。
        // 这同一个数据包还有上云台发射子弹的信息。发射命令缓冲在内部。can在接收到控制topic时即发送。发送信息是mutex保护的。
        // can 节点接收到云台电机控制电流信息后，查看是否有发射命令，如果有，发送发射命令，并且设置信息为无效。
        // 由于此电流控制信息发送速度很快，发射命令将不会延迟太久（约2ms）。（步兵发射响应速度延迟在300ms的量级）
        // 话题名称为cmd_up_yaw_current_shoot，是自定义消息类型，在auto_aim_interfaces 包下，包含两个int16_t数据和一个bool数据。分别是yaw电流，pitch电流，是否发射
        auto message = auto_aim_interfaces::msg::CmdUpYawVoltShoot();
        message.yaw_volt = gimbalYawCtrl.giveVolt;
        message.pitch_volt = gimbalPitchCtrl.giveVolt;
        std::lock_guard<std::mutex> lock(dangle_shoot_info_mutex_);
        message.shoot = shoot_;
        if(shoot_)
        {
            shoot_ = false;
        }
        cmd_up_yaw_volt_shoot_publisher_->publish(message);
        // RCLCPP_INFO(this->get_logger(), "Timer callback");
    }

    void dangle_shoot_msg_message_callback(const auto_aim_interfaces::msg::DangleShoot::SharedPtr msg)
    {
        /*
        Copilot:
        在C++中，当你使用std::lock_guard时，你不需要显式地调用解锁。std::lock_guard在构造时自动获取锁，并在析构时自动释放锁。这是一种被称为RAII（Resource Acquisition Is Initialization）的编程技术，它可以确保资源的正确释放，避免因忘记释放资源而导致的问题。
        */
        std::lock_guard<std::mutex> lock(dangle_shoot_info_mutex_);
        // 在这里更新你的数据
        delta_pitch_ = msg->delta_pitch;
        delta_yaw_ = msg->delta_yaw;
        shoot_ = msg->shoot;

        std::lock_guard<std::mutex> lockwant(want_mutex_);
        std::lock_guard<std::mutex> lockimu(imu_mutex_);
        want_pitch_ = pitch_ + delta_pitch_;
        want_yaw_ = yaw_ + delta_yaw_;
    }

    void ecd_yaw_message_callback(const std_msgs::msg::UInt16::SharedPtr msg)
    {
        #ifdef SHOW_ECD
        // log
        RCLCPP_ERROR(this->get_logger(),"The ecd is: %d", msg->data);
        #endif
        std::lock_guard<std::mutex> lock(ecd_yaw_mutex_);
        ecd_yaw_ = msg->data;
    }

    void imu_message_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {

        // 在这里更新你的数据

        // sensor_msgs::msg::Imu imu_data;
        // imu_data.orientation.w = ahrs_frame_.frame.data.data_pack.Qw;
        // imu_data.orientation.x = ahrs_frame_.frame.data.data_pack.Qx;
        // imu_data.orientation.y = ahrs_frame_.frame.data.data_pack.Qy;
        // imu_data.orientation.z = ahrs_frame_.frame.data.data_pack.Qz;
        // imu_data.angular_velocity.x = imu_frame_.frame.data.data_pack.gyroscope_x;
        // imu_data.angular_velocity.y = imu_frame_.frame.data.data_pack.gyroscope_y;
        // imu_data.angular_velocity.z = imu_frame_.frame.data.data_pack.gyroscope_z;
        // imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
        // imu_data.linear_acceleration.y = imu_frame_.frame.data.data_pack.accelerometer_y;
        // imu_data.linear_acceleration.z = imu_frame_.frame.data.data_pack.accelerometer_z;
        // 这里就认为orientation是在世界坐标系下的姿态角——旋转矩阵R1，angular_velocity是在这个姿态坐标架下的角速度。
        
        // 可以将imu的角速度看做是imu坐标系下的一个“向量”。将这个向量 v 转化到云台坐标系下的向量，就是gimbal_link在xyz方向上的角速度。是roll, pitch, yaw方向的角速度。
        // 计算方式是：R3 v ，其中，R3表示的是imu在云台link坐标架下的姿态。是程序初始化时从参数行传入的IMU rpy


        // 为了得到云台的pitch和yaw角度，需要计算 R1 R2， 其中，R2表示的是云台link坐标架在IMU坐标架下的姿态。是程序初始化时从参数行传入传入的IMU rpy的逆
        // 用一个存储

        // 从消息中获取IMU数据

        tf2::Quaternion imu_in_world(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
        tf2::Matrix3x3 r1(imu_in_world);
        tf2::Matrix3x3 r2(q_imu_gl);
        tf2::Matrix3x3 r3(q_gl_imu);

        // 将角速度转换为云台坐标系下的向量
        tf2::Vector3 v(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
        tf2::Vector3 v_gimbal_angular = r3 * v;

        // 计算云台的pitch和yaw角度
        // 两个矩阵相乘，然后得到欧拉角
        tf2::Matrix3x3 rpy_mat = r1 * r2;
        double roll, pitch, yaw;
        rpy_mat.getRPY(roll, pitch, yaw);

        // 更新数值，有互斥锁操作
        std::lock_guard<std::mutex> lock(imu_mutex_);
        v_pitch_ = v_gimbal_angular.y();
        v_yaw_ = v_gimbal_angular.z();
        pitch_ = pitch;
        yaw_ = yaw;
    }

    rclcpp::TimerBase::SharedPtr timer_;

    // 一些全局控制参数
    rclcpp::Subscription<auto_aim_interfaces::msg::DangleShoot>::SharedPtr dangle_shoot_subscription_;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr ecd_yaw_subscription_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
    
    rclcpp::Publisher<auto_aim_interfaces::msg::CmdUpYawVoltShoot>::SharedPtr cmd_up_yaw_volt_shoot_publisher_;

    std::mutex dangle_shoot_info_mutex_;
    float delta_pitch_;
    float delta_yaw_;
    bool shoot_;

    std::mutex ecd_yaw_mutex_;
    unsigned short ecd_yaw_;

    std::mutex imu_mutex_;
    float pitch_, yaw_;
    float v_pitch_, v_yaw_;

    std::mutex want_mutex_;
    float want_pitch_, want_yaw_;

    tf2::Quaternion q_gl_imu; // imu 在 gimbal link下的位姿
    tf2::Quaternion q_imu_gl; // gimbal link 在 imu系下的位姿



};
int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SenUpCtrNode>());
    rclcpp::shutdown();
    return 0;
}